#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick stick1, stick2; // only joystick

public:
	RobotDemo(void):
    /* Setup the Robot */
		myRobot(1, 2),	// these must be initialized in the same order
    /* Setup the Joysticks */
		stick1(1), stick2(2)		// as they are declared above.
	{
		myRobot.SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{	float axisvalue1[10], axisvalue2[10], x1, x2, y1, y2, z1, z2, twist1, twist2, throttle1, throttle2;
		bool buttonvalue1[20], buttonvalue2[20], trigger1, trigger2, top1, top2;
		int i;
		
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
//			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			for (i = 0; i <= 9; i++) {
				axisvalue1[i] = stick1.GetRawAxis(i+1);
				axisvalue2[i] = stick2.GetRawAxis(i+1);
			}
			
			x1 = stick1.GetX();
			x2 = stick2.GetX();
			y1 = stick1.GetY();
			y2 = stick2.GetY();
			z1 = stick1.GetZ();
			z2 = stick2.GetZ();
			twist1 = stick1.GetTwist();
			twist2 = stick2.GetTwist();
			throttle1 = stick1.GetThrottle();
			throttle2 = stick2.GetThrottle();
			
			for (i=0; i <= 19; i++) {
				buttonvalue1[i] = stick1.GetRawButton(i+1);
				buttonvalue2[i] = stick2.GetRawButton(i+1);
			}
			
			trigger1 = stick1.GetTrigger();
			trigger2 = stick2.GetTrigger();
			top1 = stick1.GetTop();
			top2 = stick2.GetTop();
						
			printf("Stick1->X-Axis %f", x1);
			printf("    Stick2->X-Axis %f\n", x2);
			printf("Stick1->Y-Axis %f", y1);
			printf("    Stick2->Y-Axis %f\n", y2);
			printf("Stick1->Z-Axis %f", z1);
			printf("    Stick2->Z-Axis %f\n", z2);
			printf("Stick1->Twist %f", twist1);
			printf("    Stick2->Twist %f\n", twist2);
			printf("Stick1->Throttle %f", throttle1);
			printf("    Stick2->Throttle %f\n", throttle2);

			for (i = 0; i <= 9; i++) {
				printf("Stick1->Axis %d",i+1);
				printf("  %f",axisvalue1[i]);
				printf("   Stick2->Axis %d",i+1);
				printf("  %f\n",axisvalue2[i]);
			}

			printf("\n");
			
			printf("Stick1->Trigger %d", trigger1);
			printf("    Stick2->Trigger %d", trigger2);
			printf("Stick1->Top Button %d", top1);
			printf("    Stick2->Top Button %d", top2);			
			
			for (i=0;  i < 19; i++){
				printf("Stick1->Button %d",i+1);
				printf("  %d",buttonvalue1[i]);
				printf("   Stick2->Button %d",i+1);
				printf("  %d\n",buttonvalue2[i]);
			}

			printf("\n\n");
			Finalize();
			Wait(0.005);				// wait for a motor update time
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

